//
// Created by 74354 on 2022/1/11.
//

#ifndef MYODRIVE_FOC_H
#define MYODRIVE_FOC_H

#include "stm32f405xx.h"
#include "math.h"
#include "focMath.h"
#include "main.h"
#include "stm32f4xx_hal_tim.h"
#include "myFliter.hpp"
#include <tuple>

#define FOC_I_DQ_SIZE 12

class FOC;
void rev_trans(FOC* f,float i_a,float i_b,float i_c,float * i_q,float* i_d);

class FOC
{
private:
    float u_dc;
    uint16_t Ts;

    float u_q;
    float u_d;
    float Tv[8];

    struct PI_Control{
        float kp_Q{} ;
        float ki_Q{} ;
        float kp_D{};
        float ki_D{};
        float i_integral_d = 0;
        float i_integral_q = 0;
    };
    struct PID_Control{
        float p_gain;
        float i_gain;
        float d_gain;
        float integral;
        float lastError;
    };

    uint8_t sectionCheck();


public:
    struct focConfig{
        float udc;
        float ts;
        odTIM timer;
        bool en_kalman;
        float (*getElecRadian)();
        float (*getMachRadian)();
        std::tuple<float,float,float> (*get_I_abc)();
    };
    PI_Control piControl;
    PID_Control posControl;
    float i_q;
    float i_d;
    uint16_t uABCTime[3];
    bool EN_KALMAN;
    TIM_HandleTypeDef driver_timer;
    myFilter::Kalman1P<float>* kalman_I_D;
    myFilter::Kalman1P<float>* kalman_I_Q;

    FOC(){};
    FOC(float udc,uint16_t ts,odTIM timer):u_dc(udc),Ts(ts),driver_timer(driver_timer){};
    void config(focConfig* config){u_dc = config->udc;Ts = config->ts;driver_timer = config->timer;EN_KALMAN = config->en_kalman;\
        getElectricRadian = config->getElecRadian;getMachineRadian = config->getMachRadian;
        getCurrentABC = config->get_I_abc;
    }

    float getTs()const{return Ts;}
    float (*getElectricRadian)();
    float (*getMachineRadian)();
    std::tuple<float,float,float> (*getCurrentABC)();

    void set_PI_control_para(float kp_q,float ki_q,float kp_d,float ki_d){piControl.kp_Q = kp_q;piControl.ki_Q = ki_q;piControl.ki_D = ki_d;piControl.kp_D = kp_d;}
    void set_posControler_para(float kp,float ki,float kd){posControl.i_gain = ki;posControl.p_gain = kp;posControl.d_gain = kd;}
    float u_alpha;
    float u_beta;
    void svpwm();
    void release();
    void runToAngle(float angle);
    void current_control(float iq,float id);
    void positionControl(float mRadianNext);
    void backward();
    void forward(float iq_ref,float id_ref);
};




#endif //MYODRIVE_FOC_H
